﻿#!/usr/bin/python3
# coding=utf-8

import math
import numpy as np
import cv2
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

# 相机参数, 59.8度640x480相机
CAM_WID,CAM_HGT = 640,480           # 重投影到的深度图尺寸
CAM_FX,CAM_FY   = 795.209,793.957   # fx/fy
CAM_CX,CAM_CY   = 332.031,231.308   # cx/cy

# 加载点云数据
pc_path = "../../Vision3D_Demos_Data/04.pc_to_redepImg/pc_rot.csv"
pc=np.genfromtxt(pc_path, delimiter=',').astype(np.float32)

# 显示加载的点云
ax = plt.figure(1).gca(projection='3d')
ax.plot(pc[:,0],pc[:,1],pc[:,2],'b.',markersize=0.1)
plt.title('point cloud')
plt.show()  


# 滤除镜头后方的点
EPS = 1.0e-16
valid = pc[:, 2] > EPS # 返回符合条件的点云
z = pc[valid, 2]

# 点云反向映射到像素坐标位置
u = np.round(pc[valid, 0] * CAM_FX/z + CAM_CX).astype(int)  # 像素纵向坐标
v = np.round(pc[valid, 1] * CAM_FY/z + CAM_CY).astype(int)  # 像素横向坐标

# 滤除超出图像尺寸的无效像素
valid = np.bitwise_and(np.bitwise_and((u>=0), (u<CAM_WID)),
                       np.bitwise_and((v>=0), (v<CAM_HGT)))
u, v, z = u[valid], v[valid], z[valid]

# 按距离填充生成深度图，近距离覆盖远距离
img_z = np.full((CAM_HGT, CAM_WID), np.inf)
for ui, vi, zi in zip(u, v, z):
    img_z[vi, ui] = min(img_z[vi, ui], zi) # 近距离像素覆盖远距离像素

# 小洞和透射消除
img_z_shift = np.array([img_z, \
                        np.roll(img_z, 1, axis=0),\
                        np.roll(img_z,-1, axis=0),\
                        np.roll(img_z, 1, axis=1),\
                        np.roll(img_z,-1, axis=1)])
img_z = np.min(img_z_shift, axis=0)


# 随机生成的dep_rot, 用于演示数据保存成csv以及加载csv文件以及显示
# np.random.seed(1234)
# dep_rot=cv2.blur(np.random.rand(CAM_HGT,CAM_WID),(50,50))
# dep_rot[dep_rot<0.5]=math.inf

# 保存重新投影生成的深度图dep_rot
redepImg_path = "../../Vision3D_Demos_Data/04.pc_to_redepImg/dep_rot.csv"
np.savetxt(redepImg_path, img_z, fmt='%.12f', delimiter=',', newline='\n')

# 加载刚保存的深度图dep_rot并显示
img=np.genfromtxt(redepImg_path, delimiter=',').astype(np.float32)
plt.imshow(img,cmap='jet')
plt.show()


